program number1
use analyse_lattice
integer no,np
type(real_8) y(nvd)   ! nvd=3
type(normalform) normal
type(damap) map
type(taylor) x2ave
real(dp) fix(nvd),prec,bet(3),phase01,phase02,phase03,phase12,phase13,mu12,mu
real(dp) k1_2,k2_2,k1k2,k1,k2
integer i,pos

call make_lattice("lattice.txt")

! Compute One-turn map and normalize it including Chromaticities
fix=0.d0;


call find_orbit_with_fpp(lattice,pos,fix)

no=6;np=4; pos=1

call init(no,1,np,0)
call alloc(y); call alloc(normal); call alloc(map);


do i=1,nvd
y(i)=fix(i)+(1.d0.mono.i)
enddo
lattice%MAGNET(2)%k(2)%kind=3
lattice%MAGNET(2)%k(2)%i=4
lattice%MAGNET(7)%k(2)%kind=3
lattice%MAGNET(7)%k(2)%i=5
lattice%MAGNET(2)%k(3)%kind=3
lattice%MAGNET(2)%k(3)%i=6

call track(y,lattice,pos)


map=y

normal=map
mu=twopi*normal%tune(1)
call print(normal%dhdj%v(1),6)


! Now computes the average of 2*x**2   i.e.  <2 x**2> in terms of invariants
call alloc(x2ave)

do i=1,2
 y(i)=fix(i)+normal%a_t%v(i)
enddo
y(3)=fix(3)+(1.d0.mono.3)



prec=1.d-10
open(unit=20,file="results_x2ave.txt")

do i=1,lattice%n
 x2ave=2*y(1)**2
 call find_average_in_circle_space(x2ave)
 write(20,*) " "
 write(20,*) i,lattice%magnet(i)%twiss(-1),lattice%magnet(i)%name
 call print(x2ave,20,prec)
 
if(i==2) then
 bet(1)=((y(1).sub.'1')**2+(y(1).sub.'01')**2)
 phase01=atan2((y(1).sub.'01'),(y(1).sub.'1'))/twopi
endif
if(i==7) then
 bet(2)=((y(1).sub.'1')**2+(y(1).sub.'01')**2)
  phase02=atan2((y(1).sub.'01'),(y(1).sub.'1'))/twopi

endif
if(i==14) then
 bet(3)=((y(1).sub.'1')**2+(y(1).sub.'01')**2)
  phase03=atan2((y(1).sub.'01'),(y(1).sub.'1'))/twopi

endif

call track(y,lattice,i,i+1)


enddo
 x2ave=2*y(1)**2
 call find_average_in_circle_space(x2ave)
  write(20,*) " "
  write(20,*) lattice%n+1,lattice%magnet(lattice%n)%twiss(0),lattice%magnet(1)%name
 call print(x2ave,20,prec)

close(20)
call kill(x2ave);

call kill(y); call kill(normal); call kill(map);

lattice%MAGNET(2)%k(2)%kind=1
lattice%MAGNET(7)%k(2)%kind=1

write(6,*) " Beta at thin Quads "
write(6,*) bet(1),bet(2)
phase12=phase02-phase01
if(phase12<0.d0) phase12=phase12+1.d0
write(6,*) phase12
mu12=phase12*twopi

k1=bet(1)/4d0/pi
write(6,*) k1

k1_2=-bet(1)**2*sin(2*mu)/(1.d0-cos(2*mu))/8.d0/twopi
k2_2=-bet(2)**2*sin(2*mu)/(1.d0-cos(2*mu))/8.d0/twopi
write(6,*) k1_2,k2_2
k1k2=-bet(1)*bet(2)*sin(2*mu)/(1.d0-cos(2*mu))*cos(2*mu12)/4.d0/twopi
k1k2=-bet(1)*bet(2)*sin(2*mu12)/4.d0/twopi+k1k2
write(6,*) k1k2
k1_2=-bet(1)**3*( sin(3*mu)/(1.d0-cos(3*mu)) +3.d0*sin(mu)/(1.d0-cos(mu))  )  /16.d0/twopi
write(6,*) " for sextupole ", k1_2


end program number1

subroutine find_average_in_circle_space(t)
use polymorphic_complextaylor
type(taylor), intent(inout):: t
type(taylorresonance) tr
external filter_ave
real(dp) filter_ave

call alloc(tr)

tr=t

call cfu(tr%cos,filter_ave,t)

call kill(tr)


end subroutine find_average_in_circle_space

  function filter_ave(j)
  use polymorphic_complextaylor
    implicit none
    real(dp) filter_ave
    integer i
    integer,dimension(:)::j

    filter_ave=1.d0
    do i=1,c_%nd
       if(j(2*i)/=j(2*i-1)) filter_ave=0.d0
    enddo

  end  function filter_ave
